#include "stdafx.h"
#include "Vector_3d.h"
//rotate point X around a unit vector W around the origin by theta
Vector_3D	rotateAroundOrigin(Vector_3D X, Vector_3D W, double theta)
{
	double u = W.x, v = W.y, w = W.z;
	double x = X.x, y = X.y, z = X.z;
	Vector_3D result;
	result.x = u*(u*x + v*y + w*z)*(1 - cos(theta)) + x * cos(theta) + (-w*y + v*z)*sin(theta);
	result.y = v*(u*x + v*y + w*z)*(1 - cos(theta)) + y * cos(theta) + (w*x - u*z)*sin(theta);
	result.z = w*(u*x + v*y + w*z)*(1 - cos(theta)) + z * cos(theta) + (-v*x + u*y)*sin(theta);
	return result;
}

Vector_3D rotateTowardZaxis(Vector_3D X, double theta)
{
	Vector_3D A;
	double r = sqrt(X.x*X.x + X.y*X.y);
	if ( abs(r) > 1e-8)
	{
		A.x = ( cos(theta)*r - sin(theta)*X.z ) * X.x / r;
		A.y = ( cos(theta)*r - sin(theta)*X.z ) * X.y / r;
		A.z = ( sin(theta)*r + cos(theta)*X.z);
	} else {
		A.x = -sin(theta);
		A.y = 0;
		A.z = cos(theta);
	}
	return A;
}

//void normal()
//{
//	ofstream fout("normal.txt");
//	std::tr1::mt19937 eng;
//	std::tr1::uniform_real<double> dist;
//	eng.seed((unsigned int) time(0));
//	for (int i = 0; i < 1000000; i ++)
//		fout<<dist(eng)<<endl;
//	fout.close();
//
//	////use Box-Miller transform
//	//srand(time(0));
//	//double x1,x2;
//	//for (int i = 0; i < 1000000; i ++)
//	//{
//	//	x1 = rand()/(double)RAND_MAX;
//	//	x2 = rand()/(double)RAND_MAX;
//	//	if (abs(x1) > 1e-8)
//	//	fout<<sqrt(-2*log(x1))*cos(2*pi*x2)<<endl;
//	//}
//}

double normal(double sigma)
{
	std::tr1::mt19937 eng;
	std::tr1::normal_distribution<double> dist;
	eng.seed((unsigned int)0);
	return dist(eng);
}


//@tc : quicksort
void quicksort(double *data,int a,int b){
	int x,y;
	double flag = data[(a+b)/2];
	double temp;
	x = a;
	y = b;
	while ( x <= y ){
		while (( data[x] < flag )&&( x<=y )) x++;
		while (( data[y] > flag )&&( x<=y )) y--;
		if ( x <= y ){
			temp = data[x];
			data[x] = data[y];
			data[y] = temp;
			x++;
			y--;
		}
	}
	if ( x < b ) quicksort(data, x, b);
	if ( y > a ) quicksort(data, a, y);
}


//@tc : find the kth min of data
double findKthMin(double* data, int low, int high, int k){
	//special case
	if ( k == 1 )
	{
		double min = INF;
		for (int i = low; i <= high; i++)
			if ( data[i] < min ) min = data[i];
		return min;
	}
	if( k == high - low + 1)
	{
		double max = -INF;
		for (int i = low; i <= high; i++)
			if ( data[i] > max ) max = data[i];
		return max;
	}

	//normal case
	int i,j;
	double t;
	i = low;
	j = high;
	t = data[low];

	while ( i < j )
	{
		while ( i < j && data[j] > t ) j--;
		if ( i < j )
		{
			data[i] = data[j];
			i++;
		}
		while ( i < j && data[i] <= t) i++;
		if ( i < j )
		{
			data[j] = data[i];
			j--;
		}
	}
	data[i] = t;

	int cnt = i - low + 1;
	if ( cnt > k )  return findKthMin(data, low, i - 1, k);
	if ( cnt < k )  return findKthMin(data, i + 1, high, k - cnt);
	return data[i];
}
